import rclpy
import signal,sys
from rclpy.node import Node
from std_msgs.msg import Int16
from sls_laserdis.laser import laserScan

class LaserDistMeasure(Node):
    def __init__(self):
        super().__init__('laser_dist_measure_node')
        self.declare_parameter('port_name', '/dev/ttyACM0')
        port_name = self.get_parameter('port_name').get_parameter_value().string_value

        self.pub = self.create_publisher(Int16,'/sls/sensors/height_laser/out/dist_measure',2)
        self.serial_port = laserScan(port_name, 115200)
        signal.signal(signal.SIGINT, self.signal_handler)
        signal.signal(signal.SIGTERM, self.signal_handler)
        self.serial_port.start_measure()
        
        self.timer = self.create_timer(0.1, self.timeCallBack)
    def timeCallBack(self):
        data = Int16()
        H_data = self.serial_port.get_distance()
        data.data = H_data
        self.pub.publish(data) 
    def signal_handler(self, signum, frame):
        """捕获 Ctrl+C 或终止信号时调用 stop_measure()"""
        self.get_logger().info(f"Received signal {signum}, stopping laser...")
        self.serial_port.stop_measure()
        self.destroy_node()  # 清理 ROS 2 节点
        rclpy.shutdown()     # 关闭 ROS 2 上下文
        sys.exit()
        
def main(args = None):
    rclpy.init(args=args)
    node = LaserDistMeasure()
    rclpy.spin(node)
    node.destroy_node(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()
